class Link:
    id = 0
    firstLocationId = 0
    secondLocationId = 0
    length = 0

    def __init__( self,id,firstId,secondId ):
        self.id = id
        self.firstLocationId = firstId
        self.secondLocationId = secondId

    def print( self ):
        print( self.id, self.firstLocationId, self.secondLocationId )

class Node:
    id = 0
    name = ""
    xPos = 0
    yPos = 0
    costSoFar = 0
    manhattanDistanceToGoal = 0
    links = []

    def __init__( self,id,name,xPos,yPos ):
        self.id = id
        self.name = name
        self.xPos = xPos
        self.yPos = yPos
        self.costSoFar = 0
        self.manhattanDistanceToGoal = 0
        self.links = []

    def print( self ):
        print( self.id, self.name, self.xPos, self.yPos )
        for link in self.links:
            link.print()

    def addLink( self, link ):
        if link.firstLocationId == self.id:
            self.links.append(link)
        elif link.secondLocationId == self.id:
            self.links.append(link)

    def getSuccessors( self ):
        nodeIds = []
        for link in self.links:
            if self.id == link.firstLocationId:
                nodeIds.append( link.secondLocationId )
            elif self.id == link.secondLocationId:
                nodeIds.append( link.firstLocationId )

        return nodeIds


class Graph:
    Nodes = []

    def __init__( self ):
        self.data = []

    def ReadLocationsFromFile( self, filename ):
        dataFile = open( filename )
        dataFile.readline()
        for dataLine in dataFile.readlines():
            dataLine = dataLine[:len(dataLine)-1]
            dataList = dataLine.split(';')
            id = int(dataList[0])
            name = dataList[1]
            xPos = int(dataList[2])
            yPos = int(dataList[3])
            self.Nodes.append( Node(id,name,xPos,yPos) )
        dataFile.close()
        del dataLine
        del dataFile

    def ReadLinksFromFile( self, filename ):
        dataFile = open( filename )
        dataFile.readline()
        for dataLine in dataFile.readlines():
            dataLine = dataLine[:len(dataLine)-1]
            dataList = dataLine.split(';')
            id = int(dataList[0])
            firstLocation = int(dataList[1])
            secondLocation = int(dataList[2])

            for node in self.Nodes:
                if firstLocation == node.id:
                    node.links.append( Link(id,firstLocation,secondLocation) )
                elif secondLocation == node.id:
                    node.links.append( Link(id,firstLocation,secondLocation) )
            del node
            

        dataFile.close()
        del dataLine
        del dataFile

        first_x = 0
        first_y = 0
        second_x = 0
        second_y = 0

        for node in self.Nodes:
            for link in node.links:
                for n in self.Nodes:
                    if n.id == link.firstLocationId:
                        first_x = n.xPos
                        first_y = n.yPos
                    elif n.id == link.secondLocationId:
                        second_x = n.xPos
                        second_y = n.yPos
                delta_x = first_x - second_x
                if delta_x < 0:
                    delta_x = -delta_x
                delta_y = first_y - second_y
                if delta_y < 0:
                    delta_y = -delta_y

                link.length = ( (delta_x ** 2) + (delta_y ** 2) ) ** (0.5)

                


    def getNode( self, name ):
        for node in self.Nodes:
            if node.name == name:
                return node
        del node

    def getNodeById( self, id ):
        for node in self.Nodes:
            if node.id == id:
                return node
        del node

    def getAllLinksFrom( self, node ):
        linksToReturn = []
        for element in self.Nodes:
            if element == node:
                for link in element.links:
                    linksToReturn.append( link )
                del link
        del element
        return linksToReturn

    def printAllNodes( self ):
        for node in self.Nodes:
            node.print()

    def getLocations( self, link ):
        nodesToReturn = []
        for node in self.Nodes:
            if node.id == link.firstLocationId:
                nodesToReturn.append( node )
            elif node.id == link.secondLocationId:
                nodesToReturn.append( node )
        del node
        return nodesToReturn

    def Reachable( self, name1, name2 ):

        if name1 != name2:
            startNode = self.getNode( name1 )
            goalNode = self.getNode( name2 )

            path = DepthFirstSearch( self, startNode, goalNode )
            if not path:
                return False
            else:
                return path

        else:
            print('StartNode and GoalNode is the same node')





def RecPath( start, end, pathTable ):
    path = []
    node = end
    while node != start:
        path.append(node)
        node = pathTable[node]
    path.append(node)
    path.reverse()
    return path

def CalculateManhattanDistanceToGoal( graph, goalNode ):
    goalX = goalNode.xPos
    goalY = goalNode.yPos
    for node in graph.Nodes:
        nodeX = node.xPos
        nodeY = node.yPos

        deltaX = nodeX - goalX
        if deltaX < 0:
            nodeX = -nodeX
        deltaY = nodeY - goalY
        if nodeY < 0:
            nodeY = -nodeY

        node.manhattanDistanceToGoal = Pythagoras( deltaX, deltaY )

def CalculateManhattanDistanceBetweenAdjacentNodes( node1, node2 ):
    deltaX = node1.xPos - node2.xPos
    if deltaX < 0:
        deltaX = -deltaX
    deltaY = node1.yPos - node2.yPos
    if deltaY < 0:
        deltaY = -deltaY
    return Pythagoras(deltaX, deltaY)

def SelectNextNode( frontier ):
    shortestDistance = 0
    thisDistance = 0
    for node in frontier:
        thisDistance = node.costSoFar + node.manhattanDistanceToGoal
        if shortestDistance != 0:
            if thisDistance < shortestDistance:
                thisDistance = shortestDistance
        else:
            shortestDistance = thisDistance
    del node

    for node in frontier:
        if node.costSoFar + node.manhattanDistanceToGoal == shortestDistance:
            return node

def DepthFirstSearch( graph, startNode, goalNode ):
    CalculateManhattanDistanceToGoal( graph, goalNode )
    frontier = [ startNode ]
    visited = []
    path = {}
    while frontier:
        node = SelectNextNode( frontier )
        frontier.remove(node)
        visited.append(node)
        if node == goalNode:
            return RecPath( startNode, goalNode, path )
        successorsId = node.getSuccessors()
        successors = []
        for next in successorsId:
            tempNode = graph.getNodeById( next )
            if tempNode not in visited:
                if tempNode not in frontier:
                    successors.append( tempNode )
        del next
        for next in successors:
            next.costSoFar = CalculateManhattanDistanceBetweenAdjacentNodes( next, node )
            path[next] = node
            frontier.append(next)
    return False

def Pythagoras( a, b ):
    first = a * a
    second = b * b
    c = (first + second) ** (0.5)
    return c


myGraph = Graph()

myGraph.ReadLocationsFromFile( 'locations.csv')
myGraph.ReadLinksFromFile( 'links.csv' )

start = 'Sorby'
end = 'Norr'

path = myGraph.Reachable( start, end )

if not path:
    print( end, ' is NOT reachable from ', start )
else:
    print( end, ' is reachable from ', start )
    print('Path:')
    for node in path:
        print( node.name )